#pragma once

//#include "utility.h"
//#include <Windows.h>
//#include <GL/GL.h>
#include <Math.h>
#include "Vector.h"


//+==================================================================+
template<typename T>
struct Vector2D
{
	T x, y;

//+------------------------------------------------------------------+
	Vector2D(){}
	Vector2D(T _x, T _y) : x(_x), y(_y)	{}
	static Vector2D FromAngle(float omega)	{	return Vector2D(cosf(omega), sinf(omega));	}

//+------------------------------------------------------------------+
	Vector2D		operator+	(const Vector2D &b)	const	{	return Vector2D(x+b.x, y+b.y);	}
	Vector2D		operator-	(const Vector2D &b)	const	{	return Vector2D(x-b.x, y-b.y);	}
	Vector2D		operator*	(const Vector2D &b)	const	{	return Vector2D(x*b.x, y*b.y);	}
	Vector2D		operator/	(const Vector2D &b)	const	{	return Vector2D(x/b.x, y/b.y);	}
	Vector2D&	operator+=	(const Vector2D &b)			{	x+=b.x;	y+=b.y;	return *this;	}
	Vector2D&	operator-=	(const Vector2D &b)			{	x-=b.x;	y-=b.y;	return *this;	}
	Vector2D&	operator*=	(const Vector2D &b)			{	x*=b.x;	y*=b.y;	return *this;	}
	Vector2D&	operator/=	(const Vector2D &b)			{	x/=b.x;	y/=b.y;	return *this;	}

	Vector2D		operator-	()							const	{	return Vector2D(-x, -y);			}
	Vector2D		operator+	(const T &b)			const	{	return Vector2D(x+b, y+b);			}
	Vector2D		operator-	(const T &b)			const	{	return Vector2D(x-b, y-b);			}
	Vector2D		operator*	(const T &b)			const	{	return Vector2D(x*b, y*b);			}
	Vector2D		operator/	(const T &b)			const	{	return Vector2D(x/b, y/b);			}
	Vector2D&	operator+=	(const T &b)					{	x+=b;		y+=b;		return *this;	}
	Vector2D&	operator-=	(const T &b)					{	x-=b;		y-=b;		return *this;	}
	Vector2D&	operator*=	(const T &b)					{	x*=b;		y*=b;		return *this;	}
	Vector2D&	operator/=	(const T &b)					{	x/=b;		y/=b;		return *this;	}
	operator		const float*()							const	{	return &x;								}
	operator		float*		()									{	return &x;								}

//+------------------------------------------------------------------+
//	dot product
	T				operator^	(const Vector2D &b)	const	{	return x*b.x + y*b.y;				}

//+------------------------------------------------------------------+
//	magnitude of cross product, e.g. sin between two unit vectors.
	T				cross			(const Vector2D &b)	const	{	return x*b.y - y*b.x;				}


//+------------------------------------------------------------------+
	T normSquared() const
	{
		return x*x + y*y;
	}
//+------------------------------------------------------------------+
	T norm() const
	{
		return sqrt(normSquared());
	}
//+------------------------------------------------------------------+
	Vector2D& normalize()
	{
		T f = norm();
//		if(f > std::numeric_limits<T>::epsilon())
		if(f)
		{
			f = 1 / f;
			x *= f;
			y *= f;
		}
		else
		{
			x = 1;
			y = 0;
		}
		return *this;
	}
//+------------------------------------------------------------------+
	Vector2D perp() const
	{
		return Vector2D(y,-x);
	}
//+------------------------------------------------------------------+
	T angle() const
	{
		return atan2(y,x);
	}

};


//+==================================================================+
//	A curve is defined by 4 points, i.e. 8 floats.
typedef Vector2D<float> Point;
typedef Vector<Point,4> Curve;


/*
//	Why doesn't the compiler align the __m128d properly?
//+==================================================================+
template<>
struct Vector2D<double>
{
	union {
	__m128d		v;
	struct	{	double x, y;	};
	};

//+------------------------------------------------------------------+
	Vector2D(){}
	Vector2D(const __m128d &_v)	:v(_v){}
	Vector2D(double _x, double _y):x(_x),y(_y){}

//+------------------------------------------------------------------+
	Vector2D		operator+	(const Vector2D &b)	const	{	return _mm_add_pd(v,b.v);						}
	Vector2D		operator-	(const Vector2D &b)	const	{	return _mm_sub_pd(v,b.v);						}
	Vector2D		operator*	(const Vector2D &b)	const	{	return _mm_mul_pd(v,b.v);						}
	Vector2D		operator/	(const Vector2D &b)	const	{	return _mm_div_pd(v,b.v);						}
	Vector2D&	operator+=	(const Vector2D &b)			{		v = _mm_add_pd(v,b.v);	return *this;	}
	Vector2D&	operator-=	(const Vector2D &b)			{		v = _mm_sub_pd(v,b.v);	return *this;	}
	Vector2D&	operator*=	(const Vector2D &b)			{		v = _mm_mul_pd(v,b.v);	return *this;	}
	Vector2D&	operator/=	(const Vector2D &b)			{		v = _mm_div_pd(v,b.v);	return *this;	}

//+------------------------------------------------------------------+
	Vector2D		operator-	()					const			{	return Vector2D(-x, -y);			}
	Vector2D		operator+	(double b)		const			{	return Vector2D(x+b, y+b);			}
	Vector2D		operator-	(double b)		const			{	return Vector2D(x-b, y-b);			}
	Vector2D		operator*	(double b)		const			{	return Vector2D(x*b, y*b);			}
	Vector2D		operator/	(double b)		const			{	return Vector2D(x/b, y/b);			}
	Vector2D&	operator+=	(double b)						{	x+=b;		y+=b;		return *this;	}
	Vector2D&	operator-=	(double b)						{	x-=b;		y-=b;		return *this;	}
	Vector2D&	operator*=	(double b)						{	x*=b;		y*=b;		return *this;	}
	Vector2D&	operator/=	(double b)						{	x/=b;		y/=b;		return *this;	}

//+------------------------------------------------------------------+
//	dot product
	double		operator^	(const Vector2D &b)	const	{	return x*b.x + y*b.y;				}


//+------------------------------------------------------------------+
	double normSquared() const
	{
		return x*x + y*y;
	}
//+------------------------------------------------------------------+
	double norm() const
	{
		return sqrt(normSquared());
	}
//+------------------------------------------------------------------+
	Vector2D& normalize()
	{
		double f = norm();
//		if(f > std::numeric_limits<double>::epsilon())
		if(f)
		{
			x /= f;
			y /= f;
		}
		return *this;
	}
//+------------------------------------------------------------------+
	Vector2D perp() const
	{
		return Vector2D(y,-x);
	}

};
*/

//+------------------------------------------------------------------+
template<typename T>	inline	T	LinearInterpolation(double t, const T &a, const T &b)			{	return a + (b - a) * t;	}
template<typename T>	inline	T	LinearInterpolation(float  t, const T &a, const T &b)			{	return a + (b - a) * t;	}

//+------------------------------------------------------------------+
inline	Point CatmullRom(float t, const Point &p0, const Point &p1, const Point &p2, const Point &p3)
{
	float	t2 = t * t,
			t3 = t2 * t;

	return p1 + (p2-p0)*(t*0.5f) + (p0 - p1*2.5f + p2*2.0f - p3*0.5f)*t2 + ((p1-p2)*1.5f + (p3-p0)*0.5f)*t3;
}

//+------------------------------------------------------------------+
inline	Point BezierCurve(float t, const Point p[4])
{
	float		t_2		= t*t,
				t_3		= t*t_2,
				_1_t		= 1.0f - t,
				_1_t_2	= _1_t*_1_t,
				_1_t_3	= _1_t*_1_t_2;
		
	return p[0]*_1_t_3 + p[1]*(3.0f*_1_t_2*t) + p[2]*(3.0f*_1_t*t_2) + p[3]*t_3;
}

//+------------------------------------------------------------------+
inline	Point BSplineCurve(float t, const Point &p1, const Point &p2, const Point &p3, const Point &p4)
{
	float		t_2		= t*t,
				t_3		= t*t_2;

	Point a[4];

	a[0] = ( (p2-p3)*3.0f + p4 - p1) / 6.0f;
	a[1] = ( (p1+p3)*3.0f - p2*6.0f) / 6.0f;
	a[2] = ( (p3-p1)*3.0f          ) / 6.0f;
	a[3] = ( p1 + p2*4.0f + p3     ) / 6.0f;

	return a[3] + (a[2] + (a[1] + a[0]*t) * t) * t;
}

//+------------------------------------------------------------------+
inline float BezierLength(const Point b[4])
{
	Vector2D<float>	p0 = (b[0] - b[1]),
							p1 = (b[2] - b[1]),
							p2,
							p3 = (b[3] - b[2]);
	float	l0 = p0.norm(),
			l1 = p1.norm(),
			l3 = p3.norm();
	if (l0 > 0.f) p0 /= l0;
	if (l1 > 0.f) p1 /= l1;
	if (l3 > 0.f) p3 /= l3;
	p2 = -p1;
	double a = abs(p0 ^ p1) + abs(p2 ^ p3);

	if((a > 1.98f) || ((l0+l1+l3) < ((4.0f - a)*8.0f)))
		return l0+l1+l3;

	Vector2D<float> bl[4],br[4];
	bl[0] = b[0];
	bl[1] = (b[0]+b[1])*0.5f;
	Vector2D<float> mid = (b[1]+b[2])*0.5f;
	bl[2] = (bl[1]+mid)*0.5f;
	br[3] = b[3];
	br[2] = (b[2]+b[3])*0.5f;
	br[1] = (br[2]+mid)*0.5f;
	br[0] = (br[1]+bl[2])*0.5f;
	bl[3] = br[0];
	return BezierLength(bl) + BezierLength(br);
}

//+==================================================================+
template<typename T>
struct Spinor
{
	Vector2D<T> c;									// complex number

//+------------------------------------------------------------------+
	Spinor(){};
	Spinor(double x, double y)				:	c(x, y)				{};
	Spinor(const Vector2D<T> &complex)	:	c(complex)			{};
	static Spinor FromAngle(T omega)
	{
//		omega *= -0.5;
		return Spinor(cos(omega), sin(omega));
	}

//+------------------------------------------------------------------+
	Spinor	operator *	(const Spinor &b) const
	{
		return Spinor(c.x*b.c.x - c.y*b.c.y, b.c.x*c.y + b.c.y*c.x);
	}

//+------------------------------------------------------------------+
	Spinor&	operator *=	(const Spinor &b)
	{
		double	x = c.x*b.c.x - c.y*b.c.y,
					y = b.c.x*c.y + b.c.y*c.x;

		c.x = x;
		c.y = y;
		return *this;
	}

//+------------------------------------------------------------------+
	operator Vector2D<T>() const
	{
		return c;
	}

//+------------------------------------------------------------------+
	Spinor	operator +	(const Spinor &b) const	{	return c + b.c;						}
	Spinor	operator -	(const Spinor &b) const	{	return c - b.c;						}
	Spinor&	operator+=	(const Spinor &b)			{			c += b.c;	return *this;	}
	Spinor&	operator-=	(const Spinor &b)			{			c -= b.c;	return *this;	}

//+------------------------------------------------------------------+
	double dot(const Spinor &b) const
	{
		return c^b.c;
	}

//+------------------------------------------------------------------+
	double norm() const
	{
		return c.norm();
	}

//+------------------------------------------------------------------+
	Spinor& normalize()
	{
		c.normalize();
		return *this;
	}

//+------------------------------------------------------------------+
//	conjugate
	Spinor	operator ~	() const
	{
		return Spinor(c.x, -c.y);
	}


//+------------------------------------------------------------------+
	Spinor	inverse() const
	{
		double f = 1.0 / c.normSquared();
		return Vector2D(f*c.x, -f*c.y);
	}

//+------------------------------------------------------------------+
	Spinor	lerp(const Spinor &b, const double t) const
	{
		Spinor lerp = c + (b.c - c) * t;
		return lerp.normalize();
	}

//+------------------------------------------------------------------+
/*	void glApply() const
	{
		double M[16] = {1,0,0,0 , 0,1,0,0 , 0,0,1,0 , 0,0,0,1};
		M[0] = c.x;   M[4] = -c.y;
		M[1] = c.y;   M[5] =  c.x;
		glMultMatrixd(M);
	}
*/
};



